

<!DOCTYPE html>
<!--[if IE 8]><html class="no-js lt-ie9" lang="en" > <![endif]-->
<!--[if gt IE 8]><!--> <html class="no-js" lang="en" > <!--<![endif]-->
<head>
  <meta charset="utf-8">
  
  <meta name="viewport" content="width=device-width, initial-scale=1.0">
  
  <title>Motion Planning API &mdash; moveit_tutorials Kinetic documentation</title>
  

  
  
    <link rel="shortcut icon" href="../../_static/favicon.ico"/>
  

  

  
  
    

  

  
  
    <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
  

  
    <link rel="stylesheet" href="../../_static/override.css" type="text/css" />
  

  
    <link rel="top" title="moveit_tutorials Kinetic documentation" href="../../index.html"/>
        <link rel="next" title="Motion Planning Pipeline" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html"/>
        <link rel="prev" title="Planning Scene ROS API" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html"/> 

  
  <script src="../../_static/js/modernizr.min.js"></script>

  <!-- Global site tag (gtag.js) - Google Analytics -->
  <script async src="https://www.googletagmanager.com/gtag/js?id=UA-108532843-1"></script>
  <script>
    window.dataLayer = window.dataLayer || [];
    function gtag(){dataLayer.push(arguments);}
    gtag('js', new Date());

    gtag('config', 'UA-108532843-1');
  </script>

</head>

<body class="wy-body-for-nav" role="document">

  <div class="wy-grid-for-nav">

    
    <nav data-toggle="wy-nav-shift" class="wy-nav-side">
      <div class="wy-side-nav-search">
        

        <a href="../../index.html">
          <img src="../../_static/logo.png"/>
          <span>Planning Framework</span>
        </a>

        
<div role="search">
  <form id="rtd-search-form" class="wy-form" action="../../search.html" method="get">
    <input type="text" name="q" placeholder="Search docs" />
    <input type="hidden" name="check_keywords" value="yes" />
    <input type="hidden" name="area" value="default" />
  </form>
</div>

        
      </div>

      <div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="main navigation">
        
          
          
              <ul>
<li class="toctree-l1"><a class="reference internal" href="../getting_started/getting_started.html">Getting Started</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../getting_started/getting_started.html#install-ros-and-catkin">Install ROS and Catkin</a></li>
<li class="toctree-l2"><a class="reference internal" href="../getting_started/getting_started.html#install-moveit">Install MoveIt!</a></li>
<li class="toctree-l2"><a class="reference internal" href="../getting_started/getting_started.html#create-a-catkin-workspace">Create A Catkin Workspace</a></li>
<li class="toctree-l2"><a class="reference internal" href="../getting_started/getting_started.html#download-the-example-code">Download the Example Code</a></li>
<li class="toctree-l2"><a class="reference internal" href="../getting_started/getting_started.html#build-your-catkin-workspace">Build your Catkin Workspace</a></li>
<li class="toctree-l2"><a class="reference internal" href="../getting_started/getting_started.html#next-step">Next Step</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html">MoveIt! Quickstart in RViz</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#step-1-launch-the-demo-and-configure-the-plugin">Step 1: Launch the Demo and Configure the Plugin</a></li>
<li class="toctree-l2"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#step-2-play-with-the-visualized-robots">Step 2: Play with the Visualized Robots</a></li>
<li class="toctree-l2"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#step-3-interact-with-the-panda">Step 3: Interact with the Panda</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#moving-into-collision">Moving into collision</a></li>
<li class="toctree-l3"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#moving-out-of-reachable-workspace">Moving out of Reachable Workspace</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#step-4-use-motion-planning-with-the-panda">Step 4: Use Motion Planning with the Panda</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#introspecting-trajectory-waypoints">Introspecting Trajectory Waypoints</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#next-steps">Next Steps</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#rviz-visual-tools">RViz Visual Tools</a></li>
<li class="toctree-l3"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#saving-your-configuration">Saving Your Configuration</a></li>
<li class="toctree-l3"><a class="reference internal" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#next-tutorials">Next Tutorials</a></li>
</ul>
</li>
</ul>
</li>
</ul>
<ul>
<li class="toctree-l1"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html">Move Group C++ Interface</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#running-the-code">Running the Code</a></li>
<li class="toctree-l2"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#expected-output">Expected Output</a></li>
<li class="toctree-l2"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#the-entire-code">The Entire Code</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#setup">Setup</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#visualization">Visualization</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#getting-basic-information">Getting Basic Information</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#start-the-demo">Start the demo</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#planning-to-a-pose-goal">Planning to a Pose goal</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#visualizing-plans">Visualizing plans</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#moving-to-a-pose-goal">Moving to a pose goal</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#planning-to-a-joint-space-goal">Planning to a joint-space goal</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#planning-with-path-constraints">Planning with Path Constraints</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#cartesian-paths">Cartesian Paths</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#adding-removing-objects-and-attaching-detaching-objects">Adding/Removing Objects and Attaching/Detaching Objects</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../move_group_interface/move_group_interface_tutorial.html#the-launch-file">The Launch File</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html">Move Group Python Interface</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#start-rviz-and-movegroup-node">Start RViz and MoveGroup node</a></li>
<li class="toctree-l2"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#expected-output">Expected Output</a></li>
<li class="toctree-l2"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#the-entire-code">The Entire Code</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#getting-basic-information">Getting Basic Information</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal">Planning to a Joint Goal</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-pose-goal">Planning to a Pose Goal</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#cartesian-paths">Cartesian Paths</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#displaying-a-trajectory">Displaying a Trajectory</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#executing-a-plan">Executing a Plan</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#adding-objects-to-the-planning-scene">Adding Objects to the Planning Scene</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#ensuring-collision-updates-are-receieved">Ensuring Collision Updates Are Receieved</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#attaching-objects-to-the-robot">Attaching Objects to the Robot</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#detaching-objects-from-the-robot">Detaching Objects from the Robot</a></li>
<li class="toctree-l3"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#removing-objects-from-the-planning-scene">Removing Objects from the Planning Scene</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../move_group_python_interface/move_group_python_interface_tutorial.html#the-launch-file">The Launch File</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../moveit_commander_scripting/moveit_commander_scripting_tutorial.html">MoveIt! Commander Scripting</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../moveit_commander_scripting/moveit_commander_scripting_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../moveit_commander_scripting/moveit_commander_scripting_tutorial.html#starting-rviz-and-the-command-line-tool">Starting RViz and the Command Line Tool</a></li>
<li class="toctree-l2"><a class="reference internal" href="../moveit_commander_scripting/moveit_commander_scripting_tutorial.html#using-the-moveit-commander-command-line-tool">Using the MoveIt! Commander Command Line Tool</a></li>
</ul>
</li>
</ul>
<ul class="current">
<li class="toctree-l1"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html">Robot Model and Robot State</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#the-robotmodel-and-robotstate-classes">The RobotModel and RobotState Classes</a></li>
<li class="toctree-l2"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#running-the-code">Running the Code</a></li>
<li class="toctree-l2"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#expected-output">Expected Output</a></li>
<li class="toctree-l2"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#the-entire-code">The Entire Code</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#start">Start</a></li>
<li class="toctree-l3"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#get-joint-values">Get Joint Values</a></li>
<li class="toctree-l3"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#joint-limits">Joint Limits</a></li>
<li class="toctree-l3"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#forward-kinematics">Forward Kinematics</a></li>
<li class="toctree-l3"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#inverse-kinematics">Inverse Kinematics</a></li>
<li class="toctree-l3"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#get-the-jacobian">Get the Jacobian</a></li>
<li class="toctree-l3"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#the-launch-file">The Launch File</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html#debugging-the-robot-state">Debugging the Robot State</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html">Planning Scene</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#the-entire-code">The entire code</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#setup">Setup</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#collision-checking">Collision Checking</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#self-collision-checking">Self-collision checking</a></li>
<li class="toctree-l4"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#change-the-state">Change the state</a></li>
<li class="toctree-l4"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#checking-for-a-group">Checking for a group</a></li>
<li class="toctree-l4"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#getting-contact-information">Getting Contact Information</a></li>
<li class="toctree-l4"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#modifying-the-allowed-collision-matrix">Modifying the Allowed Collision Matrix</a></li>
<li class="toctree-l4"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#full-collision-checking">Full Collision Checking</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#constraint-checking">Constraint Checking</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#checking-kinematic-constraints">Checking Kinematic Constraints</a></li>
<li class="toctree-l4"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#user-defined-constraints">User-defined constraints</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#the-launch-file">The launch file</a></li>
<li class="toctree-l2"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#running-the-code">Running the code</a></li>
<li class="toctree-l2"><a class="reference internal" href="../planning_scene/planning_scene_tutorial.html#expected-output">Expected Output</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html">Planning Scene ROS API</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#running-the-code">Running the code</a></li>
<li class="toctree-l2"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#expected-output">Expected Output</a></li>
<li class="toctree-l2"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#the-entire-code">The entire code</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#visualization">Visualization</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#ros-api">ROS API</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#advertise-the-required-topic">Advertise the required topic</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#define-the-attached-object-message">Define the attached object message</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#add-an-object-into-the-environment">Add an object into the environment</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#interlude-synchronous-vs-asynchronous-updates">Interlude: Synchronous vs Asynchronous updates</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#attach-an-object-to-the-robot">Attach an object to the robot</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#detach-an-object-from-the-robot">Detach an object from the robot</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#remove-the-object-from-the-collision-world">Remove the object from the collision world</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#the-launch-file">The launch file</a></li>
<li class="toctree-l2"><a class="reference internal" href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html#debugging-the-planning-scene-monitor">Debugging the Planning Scene Monitor</a></li>
</ul>
</li>
<li class="toctree-l1 current"><a class="current reference internal" href="">Motion Planning API</a><ul>
<li class="toctree-l2"><a class="reference internal" href="#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="#running-the-demo">Running the Demo</a></li>
<li class="toctree-l2"><a class="reference internal" href="#expected-output">Expected Output</a></li>
<li class="toctree-l2"><a class="reference internal" href="#the-entire-code">The Entire Code</a><ul>
<li class="toctree-l3"><a class="reference internal" href="#start">Start</a></li>
<li class="toctree-l3"><a class="reference internal" href="#visualization">Visualization</a></li>
<li class="toctree-l3"><a class="reference internal" href="#pose-goal">Pose Goal</a></li>
<li class="toctree-l3"><a class="reference internal" href="#visualize-the-result">Visualize the result</a></li>
<li class="toctree-l3"><a class="reference internal" href="#joint-space-goals">Joint Space Goals</a></li>
<li class="toctree-l3"><a class="reference internal" href="#adding-path-constraints">Adding Path Constraints</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="#the-launch-file">The Launch File</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html">Motion Planning Pipeline</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html#running-the-code">Running the Code</a></li>
<li class="toctree-l2"><a class="reference internal" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html#expected-output">Expected Output</a></li>
<li class="toctree-l2"><a class="reference internal" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html#the-entire-code">The Entire Code</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html#start">Start</a></li>
<li class="toctree-l3"><a class="reference internal" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html#visualization">Visualization</a></li>
<li class="toctree-l3"><a class="reference internal" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html#pose-goal">Pose Goal</a></li>
<li class="toctree-l3"><a class="reference internal" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html#visualize-the-result">Visualize the result</a></li>
<li class="toctree-l3"><a class="reference internal" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html#joint-space-goals">Joint Space Goals</a></li>
<li class="toctree-l3"><a class="reference internal" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html#using-a-planning-request-adapter">Using a Planning Request Adapter</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html#the-launch-file">The Launch File</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../visualizing_collisions/visualizing_collisions_tutorial.html">Visualizing Collisions</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../visualizing_collisions/visualizing_collisions_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../visualizing_collisions/visualizing_collisions_tutorial.html#running-the-code">Running the Code</a></li>
<li class="toctree-l2"><a class="reference internal" href="../visualizing_collisions/visualizing_collisions_tutorial.html#classes">Classes</a></li>
<li class="toctree-l2"><a class="reference internal" href="../visualizing_collisions/visualizing_collisions_tutorial.html#interacting">Interacting</a></li>
<li class="toctree-l2"><a class="reference internal" href="../visualizing_collisions/visualizing_collisions_tutorial.html#relevant-code">Relevant Code</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../visualizing_collisions/visualizing_collisions_tutorial.html#initializing-the-planning-scene-and-markers">Initializing the Planning Scene and Markers</a></li>
<li class="toctree-l3"><a class="reference internal" href="../visualizing_collisions/visualizing_collisions_tutorial.html#collision-requests">Collision Requests</a></li>
<li class="toctree-l3"><a class="reference internal" href="../visualizing_collisions/visualizing_collisions_tutorial.html#checking-for-collisions">Checking for Collisions</a></li>
<li class="toctree-l3"><a class="reference internal" href="../visualizing_collisions/visualizing_collisions_tutorial.html#displaying-collision-contact-points">Displaying Collision Contact Points</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../visualizing_collisions/visualizing_collisions_tutorial.html#launch-file">Launch file</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../time_parameterization/time_parameterization_tutorial.html">Time Parameterization</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../time_parameterization/time_parameterization_tutorial.html#speed-control">Speed Control</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../time_parameterization/time_parameterization_tutorial.html#from-file">From File</a></li>
<li class="toctree-l3"><a class="reference internal" href="../time_parameterization/time_parameterization_tutorial.html#during-runtime">During Runtime</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../time_parameterization/time_parameterization_tutorial.html#time-parameterization-algorithms">Time Parameterization Algorithms</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../planning_with_approximated_constraint_manifolds/planning_with_approximated_constraint_manifolds_tutorial.html">Planning with Approximated Constraint Manifolds</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../planning_with_approximated_constraint_manifolds/planning_with_approximated_constraint_manifolds_tutorial.html#creating-the-constraint-database">Creating the Constraint Database</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../planning_with_approximated_constraint_manifolds/planning_with_approximated_constraint_manifolds_tutorial.html#constraints-message">Constraints message</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_with_approximated_constraint_manifolds/planning_with_approximated_constraint_manifolds_tutorial.html#construction-options">Construction Options</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../planning_with_approximated_constraint_manifolds/planning_with_approximated_constraint_manifolds_tutorial.html#graph-size">Graph size</a></li>
<li class="toctree-l4"><a class="reference internal" href="../planning_with_approximated_constraint_manifolds/planning_with_approximated_constraint_manifolds_tutorial.html#edges">Edges</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../planning_with_approximated_constraint_manifolds/planning_with_approximated_constraint_manifolds_tutorial.html#database-construction">Database Construction</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../planning_with_approximated_constraint_manifolds/planning_with_approximated_constraint_manifolds_tutorial.html#database-loading-and-usage">Database Loading and Usage</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../pick_place/pick_place_tutorial.html">Pick and Place Tutorial</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#running-the-demo">Running The Demo</a></li>
<li class="toctree-l2"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#understanding-moveit-msgs-grasp">Understanding <tt class="docutils literal"><span class="pre">moveit_msgs::Grasp</span></tt></a></li>
<li class="toctree-l2"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#the-entire-code">The Entire Code</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#creating-environment">Creating Environment</a></li>
<li class="toctree-l3"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#pick-pipeline">Pick Pipeline</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#setting-grasp-pose">Setting grasp pose</a></li>
<li class="toctree-l4"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#setting-pre-grasp-approach">Setting pre-grasp approach</a></li>
<li class="toctree-l4"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#setting-post-grasp-retreat">Setting post-grasp retreat</a></li>
<li class="toctree-l4"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#setting-posture-of-eef-before-grasp">Setting posture of eef before grasp</a></li>
<li class="toctree-l4"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#setting-posture-of-eef-during-grasp">Setting posture of eef during grasp</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#place-pipeline">Place Pipeline</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#setting-place-location-pose">Setting place location pose</a></li>
<li class="toctree-l4"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#setting-pre-place-approach">Setting pre-place approach</a></li>
<li class="toctree-l4"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#id2">Setting post-grasp retreat</a></li>
<li class="toctree-l4"><a class="reference internal" href="../pick_place/pick_place_tutorial.html#setting-posture-of-eef-after-placing-object">Setting posture of eef after placing object</a></li>
</ul>
</li>
</ul>
</li>
</ul>
</li>
</ul>
<ul>
<li class="toctree-l1"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html">MoveIt! Setup Assistant</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#overview">Overview</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#step-1-start">Step 1: Start</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#step-2-generate-self-collision-matrix">Step 2: Generate Self-Collision Matrix</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#step-3-add-virtual-joints">Step 3: Add Virtual Joints</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#step-4-add-planning-groups">Step 4: Add Planning Groups</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#step-5-add-robot-poses">Step 5: Add Robot Poses</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#step-6-label-end-effectors">Step 6: Label End Effectors</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#step-7-add-passive-joints">Step 7: Add Passive Joints</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#step-8-3d-perception">Step 8: 3D Perception</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#step-9-gazebo-simulation">Step 9: Gazebo Simulation</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#step-10-ros-control">Step 10: ROS Control</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#step-11-add-author-information">Step 11: Add Author Information</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#step-12-generate-configuration-files">Step 12: Generate Configuration Files</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#what-s-next">What&#8217;s Next</a></li>
<li class="toctree-l2"><a class="reference internal" href="../setup_assistant/setup_assistant_tutorial.html#additional-reading">Additional Reading</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html">URDF and SRDF</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#urdf">URDF</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#urdf-resources">URDF Resources</a></li>
<li class="toctree-l3"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#important-tips">Important Tips</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#special-characters-in-joint-names">Special Characters in Joint Names</a></li>
<li class="toctree-l4"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#safety-limits">Safety Limits</a></li>
<li class="toctree-l4"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#collision-checking">Collision Checking</a></li>
<li class="toctree-l4"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#test-your-urdf">Test your URDF</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#urdf-examples">URDF Examples</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#srdf">SRDF</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#virtual-joints">Virtual Joints</a></li>
<li class="toctree-l3"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#passive-joints">Passive Joints</a></li>
<li class="toctree-l3"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#groups">Groups</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#collection-of-joints">Collection of Joints</a></li>
<li class="toctree-l4"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#collection-of-links">Collection of Links</a></li>
<li class="toctree-l4"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#serial-chain">Serial Chain</a></li>
<li class="toctree-l4"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#collection-of-sub-groups">Collection of Sub-Groups</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#end-effectors">End-Effectors</a></li>
<li class="toctree-l3"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#self-collisions">Self-Collisions</a></li>
<li class="toctree-l3"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#robot-poses">Robot Poses</a></li>
<li class="toctree-l3"><a class="reference internal" href="../urdf_srdf/urdf_srdf_tutorial.html#srdf-documentation">SRDF Documentation</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../controller_configuration/controller_configuration_tutorial.html">Low Level Controllers</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../controller_configuration/controller_configuration_tutorial.html#yaml-configuration">YAML Configuration</a></li>
<li class="toctree-l2"><a class="reference internal" href="../controller_configuration/controller_configuration_tutorial.html#followjointtrajectory-controller-interface">FollowJointTrajectory Controller Interface</a></li>
<li class="toctree-l2"><a class="reference internal" href="../controller_configuration/controller_configuration_tutorial.html#grippercommand-controller-interface">GripperCommand Controller Interface</a></li>
<li class="toctree-l2"><a class="reference internal" href="../controller_configuration/controller_configuration_tutorial.html#optional-allowed-trajectory-execution-duration-parameters">Optional Allowed Trajectory Execution Duration Parameters</a></li>
<li class="toctree-l2"><a class="reference internal" href="../controller_configuration/controller_configuration_tutorial.html#create-the-controller-launch-file">Create the Controller launch file</a></li>
<li class="toctree-l2"><a class="reference internal" href="../controller_configuration/controller_configuration_tutorial.html#debugging-information">Debugging Information</a></li>
<li class="toctree-l2"><a class="reference internal" href="../controller_configuration/controller_configuration_tutorial.html#remapping-joint-states-topic">Remapping /joint_states topic</a></li>
<li class="toctree-l2"><a class="reference internal" href="../controller_configuration/controller_configuration_tutorial.html#trajectory-execution-manager-options">Trajectory Execution Manager Options</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html">Perception Pipeline Tutorial</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#configuration">Configuration</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#yaml-configuration-file-point-cloud">YAML Configuration file (Point Cloud)</a></li>
<li class="toctree-l3"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#yaml-configuration-file-depth-map">YAML Configuration file (Depth Map)</a></li>
<li class="toctree-l3"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#update-the-launch-file">Update the launch file</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#add-the-yaml-file-to-the-launch-script">Add the YAML file to the launch script</a></li>
<li class="toctree-l4"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#octomap-configuration">Octomap Configuration</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#obstacle-avoidance">Obstacle Avoidance</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#running-the-interface">Running the Interface</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#detecting-and-adding-object-as-collision-object">Detecting and Adding Object as Collision Object</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#running-the-code">Running the Code</a></li>
<li class="toctree-l3"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#relevant-code">Relevant Code</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#perception-related">Perception Related</a></li>
<li class="toctree-l4"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#storing-relevant-cylinder-values">Storing Relevant Cylinder Values</a></li>
<li class="toctree-l4"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#extracting-location-and-height">Extracting Location and Height</a></li>
<li class="toctree-l4"><a class="reference internal" href="../perception_pipeline/perception_pipeline_tutorial.html#adding-cylinder-to-planning-scene">Adding Cylinder to Planning Scene</a></li>
</ul>
</li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../ikfast/ikfast_tutorial.html">IKFast Kinematics Solver</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#what-is-ikfast">What is IKFast?</a></li>
<li class="toctree-l2"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#moveit-ikfast">MoveIt! IKFast</a></li>
<li class="toctree-l2"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#moveit-ikfast-installation">MoveIt! IKFast Installation</a></li>
<li class="toctree-l2"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#openrave-installation">OpenRAVE Installation</a></li>
<li class="toctree-l2"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#create-collada-file-for-use-with-openrave">Create Collada File For Use With OpenRAVE</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#parameters">Parameters</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#create-ikfast-solution-cpp-file">Create IKFast Solution CPP File</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#select-ik-type">Select IK Type</a></li>
<li class="toctree-l3"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#choose-planning-group">Choose Planning Group</a></li>
<li class="toctree-l3"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#identify-link-numbers">Identify Link Numbers</a></li>
<li class="toctree-l3"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#generate-ik-solver">Generate IK Solver</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#create-plugin">Create Plugin</a></li>
<li class="toctree-l2"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#usage">Usage</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#test-the-plugin">Test the Plugin</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../ikfast/ikfast_tutorial.html#updating-the-plugin">Updating the Plugin</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../trac_ik/trac_ik_tutorial.html">TRAC-IK Kinematics Solver</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../trac_ik/trac_ik_tutorial.html#install">Install</a></li>
<li class="toctree-l2"><a class="reference internal" href="../trac_ik/trac_ik_tutorial.html#usage">Usage</a></li>
</ul>
</li>
</ul>
<ul>
<li class="toctree-l1"><a class="reference internal" href="../kinematics_configuration/kinematics_configuration_tutorial.html">Kinematics Configuration</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../kinematics_configuration/kinematics_configuration_tutorial.html#the-kinematics-yaml-file">The kinematics.yaml file</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../kinematics_configuration/kinematics_configuration_tutorial.html#parameters">Parameters</a></li>
<li class="toctree-l3"><a class="reference internal" href="../kinematics_configuration/kinematics_configuration_tutorial.html#the-kdl-kinematics-plugin">The KDL Kinematics Plugin</a></li>
<li class="toctree-l3"><a class="reference internal" href="../kinematics_configuration/kinematics_configuration_tutorial.html#the-lma-kinematics-plugin">The LMA Kinematics Plugin</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../kinematics_configuration/kinematics_configuration_tutorial.html#position-only-ik">Position Only IK</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../fake_controller_manager/fake_controller_manager_tutorial.html">Fake Controller Manager</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../fake_controller_manager/fake_controller_manager_tutorial.html#yaml-file-examples">YAML file examples</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../custom_constraint_samplers/custom_constraint_samplers_tutorial.html">Custom Constraint Samplers</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../custom_constraint_samplers/custom_constraint_samplers_tutorial.html#overview">Overview</a></li>
<li class="toctree-l2"><a class="reference internal" href="../custom_constraint_samplers/custom_constraint_samplers_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../custom_constraint_samplers/custom_constraint_samplers_tutorial.html#creating-a-constraint-sampler">Creating a constraint sampler</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../ompl_interface/ompl_interface_tutorial.html">OMPL Planner</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../ompl_interface/ompl_interface_tutorial.html#ompl-settings">OMPL Settings</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../ompl_interface/ompl_interface_tutorial.html#longest-valid-segment-fraction">Longest Valid Segment Fraction</a></li>
<li class="toctree-l3"><a class="reference internal" href="../ompl_interface/ompl_interface_tutorial.html#projection-evaluator">Projection Evaluator</a></li>
<li class="toctree-l3"><a class="reference internal" href="../ompl_interface/ompl_interface_tutorial.html#enforce-planning-in-joint-space">Enforce Planning in Joint Space</a></li>
<li class="toctree-l3"><a class="reference internal" href="../ompl_interface/ompl_interface_tutorial.html#other-settings">Other Settings</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../ompl_interface/ompl_interface_tutorial.html#smoothing-paths">Smoothing Paths</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../ompl_interface/ompl_interface_tutorial.html#determinism">Determinism</a></li>
<li class="toctree-l3"><a class="reference internal" href="../ompl_interface/ompl_interface_tutorial.html#ompl-optimization-objectives">OMPL Optimization Objectives</a></li>
<li class="toctree-l3"><a class="reference internal" href="../ompl_interface/ompl_interface_tutorial.html#post-processing-smoothing">Post-Processing Smoothing</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../chomp_planner/chomp_planner_tutorial.html">CHOMP Planner</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../chomp_planner/chomp_planner_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../chomp_planner/chomp_planner_tutorial.html#prerequisites">Prerequisites</a></li>
<li class="toctree-l2"><a class="reference internal" href="../chomp_planner/chomp_planner_tutorial.html#using-chomp-with-your-robot">Using CHOMP with Your Robot</a></li>
<li class="toctree-l2"><a class="reference internal" href="../chomp_planner/chomp_planner_tutorial.html#running-the-demo">Running the Demo</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../chomp_planner/chomp_planner_tutorial.html#running-chomp-with-obstacles-in-the-scene">Running CHOMP with Obstacles in the Scene</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../chomp_planner/chomp_planner_tutorial.html#tweaking-some-of-the-parameters-for-chomp">Tweaking some of the parameters for CHOMP</a></li>
<li class="toctree-l2"><a class="reference internal" href="../chomp_planner/chomp_planner_tutorial.html#difference-between-plans-obtained-by-chomp-and-ompl">Difference between plans obtained by CHOMP and OMPL</a></li>
<li class="toctree-l2"><a class="reference internal" href="../chomp_planner/chomp_planner_tutorial.html#using-ompl-as-a-pre-processor-for-chomp">Using OMPL as a pre-processor for CHOMP</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../stomp_planner/stomp_planner_tutorial.html">STOMP Planner</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../stomp_planner/stomp_planner_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../stomp_planner/stomp_planner_tutorial.html#prerequisites">Prerequisites</a></li>
<li class="toctree-l2"><a class="reference internal" href="../stomp_planner/stomp_planner_tutorial.html#installing-moveit-from-source">Installing MoveIt! from Source</a></li>
<li class="toctree-l2"><a class="reference internal" href="../stomp_planner/stomp_planner_tutorial.html#using-stomp-with-your-robot">Using STOMP with Your Robot</a></li>
<li class="toctree-l2"><a class="reference internal" href="../stomp_planner/stomp_planner_tutorial.html#running-the-demo">Running the Demo</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../stomp_planner/stomp_planner_tutorial.html#running-stomp-with-obstacles-in-the-scene">Running STOMP with Obstacles in the Scene</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../stomp_planner/stomp_planner_tutorial.html#tweaking-some-of-the-parameters-for-stomp">Tweaking some of the parameters for STOMP</a></li>
<li class="toctree-l2"><a class="reference internal" href="../stomp_planner/stomp_planner_tutorial.html#difference-between-plans-obtained-by-stomp-chomp-and-ompl">Difference between plans obtained by STOMP, CHOMP and OMPL</a></li>
<li class="toctree-l2"><a class="reference internal" href="../stomp_planner/stomp_planner_tutorial.html#running-stomp-unit-tests">Running STOMP Unit Tests</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../planning_adapters/planning_adapters_tutorial.html">Planning Adapter Tutorials</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../planning_adapters/planning_adapters_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../planning_adapters/planning_adapters_tutorial.html#prerequisites">Prerequisites</a></li>
<li class="toctree-l2"><a class="reference internal" href="../planning_adapters/planning_adapters_tutorial.html#installing-moveit-from-source">Installing MoveIt! from Source</a></li>
<li class="toctree-l2"><a class="reference internal" href="../planning_adapters/planning_adapters_tutorial.html#using-planning-request-adapter-with-your-motion-planner">Using Planning Request Adapter with Your Motion Planner</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../planning_adapters/planning_adapters_tutorial.html#running-ompl-as-a-pre-processor-for-chomp">Running OMPL as a pre-processor for CHOMP</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_adapters/planning_adapters_tutorial.html#running-chomp-as-a-post-processor-for-stomp">Running CHOMP as a post-processor for STOMP</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_adapters/planning_adapters_tutorial.html#running-ompl-as-a-pre-processor-for-stomp">Running OMPL as a pre-processor for STOMP</a></li>
<li class="toctree-l3"><a class="reference internal" href="../planning_adapters/planning_adapters_tutorial.html#running-stomp-as-a-post-processor-for-chomp">Running STOMP as a post-processor for CHOMP</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../planning_adapters/planning_adapters_tutorial.html#planning-insights-for-different-motion-planners-and-planners-with-planning-adapters">Planning Insights for different motion planners and planners with planning adapters</a></li>
</ul>
</li>
</ul>
<ul>
<li class="toctree-l1"><a class="reference internal" href="../joystick_control_teleoperation/joystick_control_teleoperation_tutorial.html">Joystick Control Teleoperation</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../joystick_control_teleoperation/joystick_control_teleoperation_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../joystick_control_teleoperation/joystick_control_teleoperation_tutorial.html#running-the-code">Running the Code</a></li>
<li class="toctree-l2"><a class="reference internal" href="../joystick_control_teleoperation/joystick_control_teleoperation_tutorial.html#joystick-command-mappings">Joystick Command Mappings</a></li>
<li class="toctree-l2"><a class="reference internal" href="../joystick_control_teleoperation/joystick_control_teleoperation_tutorial.html#debugging">Debugging</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../benchmarking/benchmarking_tutorial.html">Benchmarking</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../benchmarking/benchmarking_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../benchmarking/benchmarking_tutorial.html#example">Example</a></li>
<li class="toctree-l2"><a class="reference internal" href="../benchmarking/benchmarking_tutorial.html#viewing-results">Viewing Results</a></li>
<li class="toctree-l2"><a class="reference internal" href="../benchmarking/benchmarking_tutorial.html#parameters-of-the-benchmarkoptions-class">Parameters of the BenchmarkOptions Class</a></li>
<li class="toctree-l2"><a class="reference internal" href="../benchmarking/benchmarking_tutorial.html#parameters-of-the-benchmarkexecutor-class">Parameters of the BenchmarkExecutor Class</a></li>
<li class="toctree-l2"><a class="reference internal" href="../benchmarking/benchmarking_tutorial.html#benchmarking-of-different-motion-planners-chomp-stomp-and-ompl">Benchmarking of Different Motion Planners: CHOMP, STOMP and OMPL</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../benchmarking/benchmarking_tutorial.html#benchmarking-in-a-scene-without-obstacles">Benchmarking in a scene without obstacles</a></li>
<li class="toctree-l3"><a class="reference internal" href="../benchmarking/benchmarking_tutorial.html#benchmarking-in-a-scene-with-obstacles">Benchmarking in a scene with obstacles</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../tests/tests_tutorial.html">Integration/Unit Tests</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../tests/tests_tutorial.html#getting-started">Getting Started</a></li>
<li class="toctree-l2"><a class="reference internal" href="../tests/tests_tutorial.html#integration-test">Integration Test</a></li>
<li class="toctree-l2"><a class="reference internal" href="../tests/tests_tutorial.html#test-robots">Test Robots</a></li>
<li class="toctree-l2"><a class="reference internal" href="../tests/tests_tutorial.html#unit-tests">Unit Tests</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../tests/tests_tutorial.html#writing-unit-tests">Writing Unit Tests</a></li>
<li class="toctree-l3"><a class="reference internal" href="../tests/tests_tutorial.html#running-unit-tests">Running Unit Tests</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../tests/tests_tutorial.html#kinematic-tests">Kinematic Tests</a></li>
<li class="toctree-l2"><a class="reference internal" href="../tests/tests_tutorial.html#code-coverage">Code Coverage</a></li>
</ul>
</li>
</ul>

          
        
      </div>
      &nbsp;
    </nav>

    <section data-toggle="wy-nav-shift" class="wy-nav-content-wrap">

      
      <nav class="wy-nav-top" role="navigation" aria-label="top navigation">
        <i data-toggle="wy-nav-top" class="fa fa-bars"></i>
        <a href="../../index.html">moveit_tutorials</a>
      </nav>

      
      <div class="wy-nav-content">
        <div class="header-override">
          <p>
            <a href="http://moveit.ros.org">MoveIt! Website</a><br/>
          </p>
        </div>
        <div class="rst-content">
          <div role="navigation" aria-label="breadcrumbs navigation">
  <ul class="wy-breadcrumbs">
    <li class="wy-breadcrumbs-aside">
      
        
          <a href="https://github.com/ros-planning/moveit_tutorials/blob/melodic-devel/doc/motion_planning_api/motion_planning_api_tutorial.rst" class="fa fa-github"> Edit on GitHub</a>
        
      
    </li>
    <li><a href="../../index.html">Docs</a> &raquo;</li>
      
    <li>Motion Planning API</li>
  </ul>
  <hr/>
</div>
          <div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
           <div itemprop="articleBody">
             <!-- <div class="admonition note">  -->
               <!-- <p class="first admonition-title">Code Used in this Tutorial Available</p>  -->
               <!-- <p class="last">Code can be found at <a href = "https://github.com/ros-planning/moveit_tutorials">moveit_tutorials repository</a> in doc folder. Use melodic-devel branch.</p> -->
             <!-- </div>  -->
            
  <div class="section" id="motion-planning-api">
<h1>Motion Planning API<a class="headerlink" href="#motion-planning-api" title="Permalink to this headline">¶</a></h1>
<a class="reference internal image-reference" href="../../_images/motion_planning_api_tutorial_robot_move_arm_1st.png"><img alt="../../_images/motion_planning_api_tutorial_robot_move_arm_1st.png" src="../../_images/motion_planning_api_tutorial_robot_move_arm_1st.png" style="width: 700px;" /></a>
<p>In MoveIt!, the motion planners are loaded using a plugin infrastructure. This
allows MoveIt! to load motion planners at runtime. In this example, we will
run through the C++ code required to do this.</p>
<div class="section" id="getting-started">
<h2>Getting Started<a class="headerlink" href="#getting-started" title="Permalink to this headline">¶</a></h2>
<p>If you haven&#8217;t already done so, make sure you&#8217;ve completed the steps in <a class="reference external" href="../getting_started/getting_started.html">Getting Started</a>.</p>
</div>
<div class="section" id="running-the-demo">
<h2>Running the Demo<a class="headerlink" href="#running-the-demo" title="Permalink to this headline">¶</a></h2>
<p>Open two shells. In the first shell start RViz and wait for everything to finish loading:</p>
<div class="highlight-python"><div class="highlight"><pre>roslaunch panda_moveit_config demo.launch
</pre></div>
</div>
<p>In the second shell, run the launch file:</p>
<div class="highlight-python"><div class="highlight"><pre>roslaunch moveit_tutorials motion_planning_api_tutorial.launch
</pre></div>
</div>
<p><strong>Note:</strong> This tutorial uses the <strong>RvizVisualToolsGui</strong> panel to step through the demo. To add this panel to RViz, follow the instructions in the <a class="reference external" href="../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#rviz-visual-tools">Visualization Tutorial</a>.</p>
<p>After a short moment, the RViz window should appear and look similar to the one at the top of this page. To progress through each demo step either press the <strong>Next</strong> button in the <strong>RvizVisualToolsGui</strong> panel at the bottom of the screen or select <strong>Key Tool</strong> in the <strong>Tools</strong> panel at the top of the screen and then press <strong>N</strong> on your keyboard while RViz is focused.</p>
</div>
<div class="section" id="expected-output">
<h2>Expected Output<a class="headerlink" href="#expected-output" title="Permalink to this headline">¶</a></h2>
<p>In RViz, we should be able to see four trajectories being replayed eventually:</p>
<blockquote>
<div><ol class="arabic">
<li><p class="first">The robot moves its arm to the first pose goal,</p>
<p><a class="reference internal" href="../../_images/motion_planning_api_tutorial_robot_move_arm_1st.png"><img alt="A" src="../../_images/motion_planning_api_tutorial_robot_move_arm_1st.png" style="width: 200px;" /></a></p>
</li>
<li><p class="first">The robot moves its arm to the joint goal,</p>
<p><a class="reference internal" href="../../_images/motion_planning_api_tutorial_robot_move_arm_2nd.png"><img alt="B" src="../../_images/motion_planning_api_tutorial_robot_move_arm_2nd.png" style="width: 200px;" /></a></p>
</li>
<li><p class="first">The robot moves its arm back to the original pose goal,</p>
</li>
<li><p class="first">The robot moves its arm to a new pose goal while maintaining the end-effector level.</p>
<p><a class="reference internal" href="../../_images/motion_planning_api_tutorial_robot_move_arm_3rd.png"><img alt="C" src="../../_images/motion_planning_api_tutorial_robot_move_arm_3rd.png" style="width: 200px;" /></a></p>
</li>
</ol>
</div></blockquote>
</div>
<div class="section" id="the-entire-code">
<h2>The Entire Code<a class="headerlink" href="#the-entire-code" title="Permalink to this headline">¶</a></h2>
<p>The entire code can be seen <a class="reference external" href="https://github.com/ros-planning/moveit_tutorials/tree/melodic-devel/doc/motion_planning_api">here in the moveit_tutorials GitHub project</a>.</p>
<div class="section" id="start">
<h3>Start<a class="headerlink" href="#start" title="Permalink to this headline">¶</a></h3>
<p>Setting up to start using a planner is pretty easy. Planners are
setup as plugins in MoveIt! and you can use the ROS pluginlib
interface to load any planner that you want to use. Before we
can load the planner, we need two objects, a RobotModel and a
PlanningScene. We will start by instantiating a <a class="reference external" href="http://docs.ros.org/indigo/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html">RobotModelLoader</a>
object, which will look up the robot description on the ROS
parameter server and construct a <a class="reference external" href="http://docs.ros.org/melodic/api/moveit_core/html/classmoveit_1_1core_1_1RobotModel.html">RobotModel</a> for us
to use.</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="k">const</span> <span class="n">std</span><span class="o">::</span><span class="n">string</span> <span class="n">PLANNING_GROUP</span> <span class="o">=</span> <span class="s">&quot;panda_arm&quot;</span><span class="p">;</span>
<span class="n">robot_model_loader</span><span class="o">::</span><span class="n">RobotModelLoaderPtr</span> <span class="n">robot_model_loader</span><span class="p">(</span>
    <span class="k">new</span> <span class="n">robot_model_loader</span><span class="o">::</span><span class="n">RobotModelLoader</span><span class="p">(</span><span class="s">&quot;robot_description&quot;</span><span class="p">));</span>
<span class="n">robot_model</span><span class="o">::</span><span class="n">RobotModelPtr</span> <span class="n">robot_model</span> <span class="o">=</span> <span class="n">robot_model_loader</span><span class="o">-&gt;</span><span class="n">getModel</span><span class="p">();</span>
<span class="cm">/* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/</span>
<span class="n">robot_state</span><span class="o">::</span><span class="n">RobotStatePtr</span> <span class="n">robot_state</span><span class="p">(</span><span class="k">new</span> <span class="n">robot_state</span><span class="o">::</span><span class="n">RobotState</span><span class="p">(</span><span class="n">robot_model</span><span class="p">));</span>
<span class="k">const</span> <span class="n">robot_state</span><span class="o">::</span><span class="n">JointModelGroup</span><span class="o">*</span> <span class="n">joint_model_group</span> <span class="o">=</span> <span class="n">robot_state</span><span class="o">-&gt;</span><span class="n">getJointModelGroup</span><span class="p">(</span><span class="n">PLANNING_GROUP</span><span class="p">);</span>
</pre></div>
</div>
<p>Using the <a class="reference external" href="http://docs.ros.org/melodic/api/moveit_core/html/classmoveit_1_1core_1_1RobotModel.html">RobotModel</a>, we can construct a
<a class="reference external" href="http://docs.ros.org/melodic/api/moveit_core/html/classplanning__scene_1_1PlanningScene.html">PlanningScene</a> that maintains the state of
the world (including the robot).</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">planning_scene</span><span class="o">::</span><span class="n">PlanningScenePtr</span> <span class="n">planning_scene</span><span class="p">(</span><span class="k">new</span> <span class="n">planning_scene</span><span class="o">::</span><span class="n">PlanningScene</span><span class="p">(</span><span class="n">robot_model</span><span class="p">));</span>
</pre></div>
</div>
<p>With the planning scene we create a planing scene monitor that
monitors planning scene diffs and applys them to the planning scene</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">planning_scene_monitor</span><span class="o">::</span><span class="n">PlanningSceneMonitorPtr</span> <span class="n">psm</span><span class="p">(</span>
    <span class="k">new</span> <span class="n">planning_scene_monitor</span><span class="o">::</span><span class="n">PlanningSceneMonitor</span><span class="p">(</span><span class="n">planning_scene</span><span class="p">,</span> <span class="n">robot_model_loader</span><span class="p">));</span>
<span class="n">psm</span><span class="o">-&gt;</span><span class="n">startPublishingPlanningScene</span><span class="p">(</span><span class="n">planning_scene_monitor</span><span class="o">::</span><span class="n">PlanningSceneMonitor</span><span class="o">::</span><span class="n">UPDATE_SCENE</span><span class="p">);</span>
<span class="n">psm</span><span class="o">-&gt;</span><span class="n">startStateMonitor</span><span class="p">();</span>
<span class="n">psm</span><span class="o">-&gt;</span><span class="n">startSceneMonitor</span><span class="p">();</span>

<span class="k">while</span> <span class="p">(</span><span class="o">!</span><span class="n">psm</span><span class="o">-&gt;</span><span class="n">getStateMonitor</span><span class="p">()</span><span class="o">-&gt;</span><span class="n">haveCompleteState</span><span class="p">()</span> <span class="o">&amp;&amp;</span> <span class="n">ros</span><span class="o">::</span><span class="n">ok</span><span class="p">())</span>
<span class="p">{</span>
  <span class="n">ROS_INFO_STREAM_THROTTLE_NAMED</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="n">node_name</span><span class="p">,</span> <span class="s">&quot;Waiting for complete state from topic &quot;</span><span class="p">);</span>
<span class="p">}</span>
</pre></div>
</div>
<p>We will now construct a loader to load a planner, by name.
Note that we are using the ROS pluginlib library here.</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">boost</span><span class="o">::</span><span class="n">scoped_ptr</span><span class="o">&lt;</span><span class="n">pluginlib</span><span class="o">::</span><span class="n">ClassLoader</span><span class="o">&lt;</span><span class="n">planning_interface</span><span class="o">::</span><span class="n">PlannerManager</span><span class="o">&gt;&gt;</span> <span class="n">planner_plugin_loader</span><span class="p">;</span>
<span class="n">planning_interface</span><span class="o">::</span><span class="n">PlannerManagerPtr</span> <span class="n">planner_instance</span><span class="p">;</span>
<span class="n">std</span><span class="o">::</span><span class="n">string</span> <span class="n">planner_plugin_name</span><span class="p">;</span>
</pre></div>
</div>
<p>We will get the name of planning plugin we want to load
from the ROS parameter server, and then load the planner
making sure to catch all exceptions.</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="k">if</span> <span class="p">(</span><span class="o">!</span><span class="n">node_handle</span><span class="p">.</span><span class="n">getParam</span><span class="p">(</span><span class="s">&quot;planning_plugin&quot;</span><span class="p">,</span> <span class="n">planner_plugin_name</span><span class="p">))</span>
  <span class="n">ROS_FATAL_STREAM</span><span class="p">(</span><span class="s">&quot;Could not find planner plugin name&quot;</span><span class="p">);</span>
<span class="n">try</span>
<span class="p">{</span>
  <span class="n">planner_plugin_loader</span><span class="p">.</span><span class="n">reset</span><span class="p">(</span><span class="k">new</span> <span class="n">pluginlib</span><span class="o">::</span><span class="n">ClassLoader</span><span class="o">&lt;</span><span class="n">planning_interface</span><span class="o">::</span><span class="n">PlannerManager</span><span class="o">&gt;</span><span class="p">(</span>
      <span class="s">&quot;moveit_core&quot;</span><span class="p">,</span> <span class="s">&quot;planning_interface::PlannerManager&quot;</span><span class="p">));</span>
<span class="p">}</span>
<span class="k">catch</span> <span class="p">(</span><span class="n">pluginlib</span><span class="o">::</span><span class="n">PluginlibException</span><span class="o">&amp;</span> <span class="n">ex</span><span class="p">)</span>
<span class="p">{</span>
  <span class="n">ROS_FATAL_STREAM</span><span class="p">(</span><span class="s">&quot;Exception while creating planning plugin loader &quot;</span> <span class="o">&lt;&lt;</span> <span class="n">ex</span><span class="p">.</span><span class="n">what</span><span class="p">());</span>
<span class="p">}</span>
<span class="n">try</span>
<span class="p">{</span>
  <span class="n">planner_instance</span><span class="p">.</span><span class="n">reset</span><span class="p">(</span><span class="n">planner_plugin_loader</span><span class="o">-&gt;</span><span class="n">createUnmanagedInstance</span><span class="p">(</span><span class="n">planner_plugin_name</span><span class="p">));</span>
  <span class="k">if</span> <span class="p">(</span><span class="o">!</span><span class="n">planner_instance</span><span class="o">-&gt;</span><span class="n">initialize</span><span class="p">(</span><span class="n">robot_model</span><span class="p">,</span> <span class="n">node_handle</span><span class="p">.</span><span class="n">getNamespace</span><span class="p">()))</span>
    <span class="n">ROS_FATAL_STREAM</span><span class="p">(</span><span class="s">&quot;Could not initialize planner instance&quot;</span><span class="p">);</span>
  <span class="n">ROS_INFO_STREAM</span><span class="p">(</span><span class="s">&quot;Using planning interface &#39;&quot;</span> <span class="o">&lt;&lt;</span> <span class="n">planner_instance</span><span class="o">-&gt;</span><span class="n">getDescription</span><span class="p">()</span> <span class="o">&lt;&lt;</span> <span class="s">&quot;&#39;&quot;</span><span class="p">);</span>
<span class="p">}</span>
<span class="k">catch</span> <span class="p">(</span><span class="n">pluginlib</span><span class="o">::</span><span class="n">PluginlibException</span><span class="o">&amp;</span> <span class="n">ex</span><span class="p">)</span>
<span class="p">{</span>
  <span class="k">const</span> <span class="n">std</span><span class="o">::</span><span class="n">vector</span><span class="o">&lt;</span><span class="n">std</span><span class="o">::</span><span class="n">string</span><span class="o">&gt;&amp;</span> <span class="n">classes</span> <span class="o">=</span> <span class="n">planner_plugin_loader</span><span class="o">-&gt;</span><span class="n">getDeclaredClasses</span><span class="p">();</span>
  <span class="n">std</span><span class="o">::</span><span class="n">stringstream</span> <span class="n">ss</span><span class="p">;</span>
  <span class="k">for</span> <span class="p">(</span><span class="n">std</span><span class="o">::</span><span class="kt">size_t</span> <span class="n">i</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="n">i</span> <span class="o">&lt;</span> <span class="n">classes</span><span class="p">.</span><span class="n">size</span><span class="p">();</span> <span class="o">++</span><span class="n">i</span><span class="p">)</span>
    <span class="n">ss</span> <span class="o">&lt;&lt;</span> <span class="n">classes</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">&lt;&lt;</span> <span class="s">&quot; &quot;</span><span class="p">;</span>
  <span class="n">ROS_ERROR_STREAM</span><span class="p">(</span><span class="s">&quot;Exception while loading planner &#39;&quot;</span> <span class="o">&lt;&lt;</span> <span class="n">planner_plugin_name</span> <span class="o">&lt;&lt;</span> <span class="s">&quot;&#39;: &quot;</span> <span class="o">&lt;&lt;</span> <span class="n">ex</span><span class="p">.</span><span class="n">what</span><span class="p">()</span> <span class="o">&lt;&lt;</span> <span class="n">std</span><span class="o">::</span><span class="n">endl</span>
                                                       <span class="o">&lt;&lt;</span> <span class="s">&quot;Available plugins: &quot;</span> <span class="o">&lt;&lt;</span> <span class="n">ss</span><span class="p">.</span><span class="n">str</span><span class="p">());</span>
<span class="p">}</span>
</pre></div>
</div>
</div>
<div class="section" id="visualization">
<h3>Visualization<a class="headerlink" href="#visualization" title="Permalink to this headline">¶</a></h3>
<p>The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="k">namespace</span> <span class="n">rvt</span> <span class="o">=</span> <span class="n">rviz_visual_tools</span><span class="p">;</span>
<span class="n">moveit_visual_tools</span><span class="o">::</span><span class="n">MoveItVisualTools</span> <span class="n">visual_tools</span><span class="p">(</span><span class="s">&quot;panda_link0&quot;</span><span class="p">,</span> <span class="n">rviz_visual_tools</span><span class="o">::</span><span class="n">RVIZ_MARKER_TOPIC</span><span class="p">,</span> <span class="n">psm</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">loadRobotStatePub</span><span class="p">(</span><span class="s">&quot;/display_robot_state&quot;</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">enableBatchPublishing</span><span class="p">();</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">deleteAllMarkers</span><span class="p">();</span>  <span class="c1">// clear all old markers</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">trigger</span><span class="p">();</span>

<span class="cm">/* Remote control is an introspection tool that allows users to step through a high level script</span>
<span class="cm">   via buttons and keyboard shortcuts in RViz */</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">loadRemoteControl</span><span class="p">();</span>

<span class="cm">/* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/</span>
<span class="n">Eigen</span><span class="o">::</span><span class="n">Isometry3d</span> <span class="n">text_pose</span> <span class="o">=</span> <span class="n">Eigen</span><span class="o">::</span><span class="n">Isometry3d</span><span class="o">::</span><span class="n">Identity</span><span class="p">();</span>
<span class="n">text_pose</span><span class="p">.</span><span class="n">translation</span><span class="p">().</span><span class="n">z</span><span class="p">()</span> <span class="o">=</span> <span class="mf">1.75</span><span class="p">;</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">publishText</span><span class="p">(</span><span class="n">text_pose</span><span class="p">,</span> <span class="s">&quot;Motion Planning API Demo&quot;</span><span class="p">,</span> <span class="n">rvt</span><span class="o">::</span><span class="n">WHITE</span><span class="p">,</span> <span class="n">rvt</span><span class="o">::</span><span class="n">XLARGE</span><span class="p">);</span>

<span class="cm">/* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">trigger</span><span class="p">();</span>

<span class="cm">/* We can also use visual_tools to wait for user input */</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">prompt</span><span class="p">(</span><span class="s">&quot;Press &#39;next&#39; in the RvizVisualToolsGui window to start the demo&quot;</span><span class="p">);</span>
</pre></div>
</div>
</div>
<div class="section" id="pose-goal">
<h3>Pose Goal<a class="headerlink" href="#pose-goal" title="Permalink to this headline">¶</a></h3>
<p>We will now create a motion plan request for the arm of the Panda
specifying the desired pose of the end-effector as input.</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">visual_tools</span><span class="p">.</span><span class="n">publishRobotState</span><span class="p">(</span><span class="n">planning_scene</span><span class="o">-&gt;</span><span class="n">getCurrentStateNonConst</span><span class="p">(),</span> <span class="n">rviz_visual_tools</span><span class="o">::</span><span class="n">GREEN</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">trigger</span><span class="p">();</span>
<span class="n">planning_interface</span><span class="o">::</span><span class="n">MotionPlanRequest</span> <span class="n">req</span><span class="p">;</span>
<span class="n">planning_interface</span><span class="o">::</span><span class="n">MotionPlanResponse</span> <span class="n">res</span><span class="p">;</span>
<span class="n">geometry_msgs</span><span class="o">::</span><span class="n">PoseStamped</span> <span class="n">pose</span><span class="p">;</span>
<span class="n">pose</span><span class="p">.</span><span class="n">header</span><span class="p">.</span><span class="n">frame_id</span> <span class="o">=</span> <span class="s">&quot;panda_link0&quot;</span><span class="p">;</span>
<span class="n">pose</span><span class="p">.</span><span class="n">pose</span><span class="p">.</span><span class="n">position</span><span class="p">.</span><span class="n">x</span> <span class="o">=</span> <span class="mf">0.3</span><span class="p">;</span>
<span class="n">pose</span><span class="p">.</span><span class="n">pose</span><span class="p">.</span><span class="n">position</span><span class="p">.</span><span class="n">y</span> <span class="o">=</span> <span class="mf">0.4</span><span class="p">;</span>
<span class="n">pose</span><span class="p">.</span><span class="n">pose</span><span class="p">.</span><span class="n">position</span><span class="p">.</span><span class="n">z</span> <span class="o">=</span> <span class="mf">0.75</span><span class="p">;</span>
<span class="n">pose</span><span class="p">.</span><span class="n">pose</span><span class="p">.</span><span class="n">orientation</span><span class="p">.</span><span class="n">w</span> <span class="o">=</span> <span class="mf">1.0</span><span class="p">;</span>
</pre></div>
</div>
<p>A tolerance of 0.01 m is specified in position
and 0.01 radians in orientation</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">std</span><span class="o">::</span><span class="n">vector</span><span class="o">&lt;</span><span class="kt">double</span><span class="o">&gt;</span> <span class="n">tolerance_pose</span><span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mf">0.01</span><span class="p">);</span>
<span class="n">std</span><span class="o">::</span><span class="n">vector</span><span class="o">&lt;</span><span class="kt">double</span><span class="o">&gt;</span> <span class="n">tolerance_angle</span><span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mf">0.01</span><span class="p">);</span>
</pre></div>
</div>
<p>We will create the request as a constraint using a helper function available
from the
<a class="reference external" href="http://docs.ros.org/indigo/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132">kinematic_constraints</a>
package.</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">moveit_msgs</span><span class="o">::</span><span class="n">Constraints</span> <span class="n">pose_goal</span> <span class="o">=</span>
    <span class="n">kinematic_constraints</span><span class="o">::</span><span class="n">constructGoalConstraints</span><span class="p">(</span><span class="s">&quot;panda_link8&quot;</span><span class="p">,</span> <span class="n">pose</span><span class="p">,</span> <span class="n">tolerance_pose</span><span class="p">,</span> <span class="n">tolerance_angle</span><span class="p">);</span>

<span class="n">req</span><span class="p">.</span><span class="n">group_name</span> <span class="o">=</span> <span class="n">PLANNING_GROUP</span><span class="p">;</span>
<span class="n">req</span><span class="p">.</span><span class="n">goal_constraints</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="n">pose_goal</span><span class="p">);</span>
</pre></div>
</div>
<p>We now construct a planning context that encapsulate the scene,
the request and the response. We call the planner using this
planning context</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">planning_interface</span><span class="o">::</span><span class="n">PlanningContextPtr</span> <span class="n">context</span> <span class="o">=</span>
    <span class="n">planner_instance</span><span class="o">-&gt;</span><span class="n">getPlanningContext</span><span class="p">(</span><span class="n">planning_scene</span><span class="p">,</span> <span class="n">req</span><span class="p">,</span> <span class="n">res</span><span class="p">.</span><span class="n">error_code_</span><span class="p">);</span>
<span class="n">context</span><span class="o">-&gt;</span><span class="n">solve</span><span class="p">(</span><span class="n">res</span><span class="p">);</span>
<span class="k">if</span> <span class="p">(</span><span class="n">res</span><span class="p">.</span><span class="n">error_code_</span><span class="p">.</span><span class="n">val</span> <span class="o">!=</span> <span class="n">res</span><span class="p">.</span><span class="n">error_code_</span><span class="p">.</span><span class="n">SUCCESS</span><span class="p">)</span>
<span class="p">{</span>
  <span class="n">ROS_ERROR</span><span class="p">(</span><span class="s">&quot;Could not compute plan successfully&quot;</span><span class="p">);</span>
  <span class="k">return</span> <span class="mi">0</span><span class="p">;</span>
<span class="p">}</span>
</pre></div>
</div>
</div>
<div class="section" id="visualize-the-result">
<h3>Visualize the result<a class="headerlink" href="#visualize-the-result" title="Permalink to this headline">¶</a></h3>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">ros</span><span class="o">::</span><span class="n">Publisher</span> <span class="n">display_publisher</span> <span class="o">=</span>
    <span class="n">node_handle</span><span class="p">.</span><span class="n">advertise</span><span class="o">&lt;</span><span class="n">moveit_msgs</span><span class="o">::</span><span class="n">DisplayTrajectory</span><span class="o">&gt;</span><span class="p">(</span><span class="s">&quot;/move_group/display_planned_path&quot;</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="nb">true</span><span class="p">);</span>
<span class="n">moveit_msgs</span><span class="o">::</span><span class="n">DisplayTrajectory</span> <span class="n">display_trajectory</span><span class="p">;</span>

<span class="cm">/* Visualize the trajectory */</span>
<span class="n">moveit_msgs</span><span class="o">::</span><span class="n">MotionPlanResponse</span> <span class="n">response</span><span class="p">;</span>
<span class="n">res</span><span class="p">.</span><span class="n">getMessage</span><span class="p">(</span><span class="n">response</span><span class="p">);</span>

<span class="n">display_trajectory</span><span class="p">.</span><span class="n">trajectory_start</span> <span class="o">=</span> <span class="n">response</span><span class="p">.</span><span class="n">trajectory_start</span><span class="p">;</span>
<span class="n">display_trajectory</span><span class="p">.</span><span class="n">trajectory</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="n">response</span><span class="p">.</span><span class="n">trajectory</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">publishTrajectoryLine</span><span class="p">(</span><span class="n">display_trajectory</span><span class="p">.</span><span class="n">trajectory</span><span class="p">.</span><span class="n">back</span><span class="p">(),</span> <span class="n">joint_model_group</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">trigger</span><span class="p">();</span>
<span class="n">display_publisher</span><span class="p">.</span><span class="n">publish</span><span class="p">(</span><span class="n">display_trajectory</span><span class="p">);</span>

<span class="cm">/* Set the state in the planning scene to the final state of the last plan */</span>
<span class="n">robot_state</span><span class="o">-&gt;</span><span class="n">setJointGroupPositions</span><span class="p">(</span><span class="n">joint_model_group</span><span class="p">,</span> <span class="n">response</span><span class="p">.</span><span class="n">trajectory</span><span class="p">.</span><span class="n">joint_trajectory</span><span class="p">.</span><span class="n">points</span><span class="p">.</span><span class="n">back</span><span class="p">().</span><span class="n">positions</span><span class="p">);</span>
<span class="n">planning_scene</span><span class="o">-&gt;</span><span class="n">setCurrentState</span><span class="p">(</span><span class="o">*</span><span class="n">robot_state</span><span class="p">.</span><span class="n">get</span><span class="p">());</span>
</pre></div>
</div>
<p>Display the goal state</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">visual_tools</span><span class="p">.</span><span class="n">publishRobotState</span><span class="p">(</span><span class="n">planning_scene</span><span class="o">-&gt;</span><span class="n">getCurrentStateNonConst</span><span class="p">(),</span> <span class="n">rviz_visual_tools</span><span class="o">::</span><span class="n">GREEN</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">publishAxisLabeled</span><span class="p">(</span><span class="n">pose</span><span class="p">.</span><span class="n">pose</span><span class="p">,</span> <span class="s">&quot;goal_1&quot;</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">publishText</span><span class="p">(</span><span class="n">text_pose</span><span class="p">,</span> <span class="s">&quot;Pose Goal (1)&quot;</span><span class="p">,</span> <span class="n">rvt</span><span class="o">::</span><span class="n">WHITE</span><span class="p">,</span> <span class="n">rvt</span><span class="o">::</span><span class="n">XLARGE</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">trigger</span><span class="p">();</span>

<span class="cm">/* We can also use visual_tools to wait for user input */</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">prompt</span><span class="p">(</span><span class="s">&quot;Press &#39;next&#39; in the RvizVisualToolsGui window to continue the demo&quot;</span><span class="p">);</span>
</pre></div>
</div>
</div>
<div class="section" id="joint-space-goals">
<h3>Joint Space Goals<a class="headerlink" href="#joint-space-goals" title="Permalink to this headline">¶</a></h3>
<p>Now, setup a joint space goal</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">robot_state</span><span class="o">::</span><span class="n">RobotState</span> <span class="n">goal_state</span><span class="p">(</span><span class="n">robot_model</span><span class="p">);</span>
<span class="n">std</span><span class="o">::</span><span class="n">vector</span><span class="o">&lt;</span><span class="kt">double</span><span class="o">&gt;</span> <span class="n">joint_values</span> <span class="o">=</span> <span class="p">{</span> <span class="o">-</span><span class="mf">1.0</span><span class="p">,</span> <span class="mf">0.7</span><span class="p">,</span> <span class="mf">0.7</span><span class="p">,</span> <span class="o">-</span><span class="mf">1.5</span><span class="p">,</span> <span class="o">-</span><span class="mf">0.7</span><span class="p">,</span> <span class="mf">2.0</span><span class="p">,</span> <span class="mf">0.0</span> <span class="p">};</span>
<span class="n">goal_state</span><span class="p">.</span><span class="n">setJointGroupPositions</span><span class="p">(</span><span class="n">joint_model_group</span><span class="p">,</span> <span class="n">joint_values</span><span class="p">);</span>
<span class="n">moveit_msgs</span><span class="o">::</span><span class="n">Constraints</span> <span class="n">joint_goal</span> <span class="o">=</span> <span class="n">kinematic_constraints</span><span class="o">::</span><span class="n">constructGoalConstraints</span><span class="p">(</span><span class="n">goal_state</span><span class="p">,</span> <span class="n">joint_model_group</span><span class="p">);</span>
<span class="n">req</span><span class="p">.</span><span class="n">goal_constraints</span><span class="p">.</span><span class="n">clear</span><span class="p">();</span>
<span class="n">req</span><span class="p">.</span><span class="n">goal_constraints</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="n">joint_goal</span><span class="p">);</span>
</pre></div>
</div>
<p>Call the planner and visualize the trajectory</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="cm">/* Re-construct the planning context */</span>
<span class="n">context</span> <span class="o">=</span> <span class="n">planner_instance</span><span class="o">-&gt;</span><span class="n">getPlanningContext</span><span class="p">(</span><span class="n">planning_scene</span><span class="p">,</span> <span class="n">req</span><span class="p">,</span> <span class="n">res</span><span class="p">.</span><span class="n">error_code_</span><span class="p">);</span>
<span class="cm">/* Call the Planner */</span>
<span class="n">context</span><span class="o">-&gt;</span><span class="n">solve</span><span class="p">(</span><span class="n">res</span><span class="p">);</span>
<span class="cm">/* Check that the planning was successful */</span>
<span class="k">if</span> <span class="p">(</span><span class="n">res</span><span class="p">.</span><span class="n">error_code_</span><span class="p">.</span><span class="n">val</span> <span class="o">!=</span> <span class="n">res</span><span class="p">.</span><span class="n">error_code_</span><span class="p">.</span><span class="n">SUCCESS</span><span class="p">)</span>
<span class="p">{</span>
  <span class="n">ROS_ERROR</span><span class="p">(</span><span class="s">&quot;Could not compute plan successfully&quot;</span><span class="p">);</span>
  <span class="k">return</span> <span class="mi">0</span><span class="p">;</span>
<span class="p">}</span>
<span class="cm">/* Visualize the trajectory */</span>
<span class="n">res</span><span class="p">.</span><span class="n">getMessage</span><span class="p">(</span><span class="n">response</span><span class="p">);</span>
<span class="n">display_trajectory</span><span class="p">.</span><span class="n">trajectory</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="n">response</span><span class="p">.</span><span class="n">trajectory</span><span class="p">);</span>

<span class="cm">/* Now you should see two planned trajectories in series*/</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">publishTrajectoryLine</span><span class="p">(</span><span class="n">display_trajectory</span><span class="p">.</span><span class="n">trajectory</span><span class="p">.</span><span class="n">back</span><span class="p">(),</span> <span class="n">joint_model_group</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">trigger</span><span class="p">();</span>
<span class="n">display_publisher</span><span class="p">.</span><span class="n">publish</span><span class="p">(</span><span class="n">display_trajectory</span><span class="p">);</span>

<span class="cm">/* We will add more goals. But first, set the state in the planning</span>
<span class="cm">   scene to the final state of the last plan */</span>
<span class="n">robot_state</span><span class="o">-&gt;</span><span class="n">setJointGroupPositions</span><span class="p">(</span><span class="n">joint_model_group</span><span class="p">,</span> <span class="n">response</span><span class="p">.</span><span class="n">trajectory</span><span class="p">.</span><span class="n">joint_trajectory</span><span class="p">.</span><span class="n">points</span><span class="p">.</span><span class="n">back</span><span class="p">().</span><span class="n">positions</span><span class="p">);</span>
<span class="n">planning_scene</span><span class="o">-&gt;</span><span class="n">setCurrentState</span><span class="p">(</span><span class="o">*</span><span class="n">robot_state</span><span class="p">.</span><span class="n">get</span><span class="p">());</span>
</pre></div>
</div>
<p>Display the goal state</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">visual_tools</span><span class="p">.</span><span class="n">publishRobotState</span><span class="p">(</span><span class="n">planning_scene</span><span class="o">-&gt;</span><span class="n">getCurrentStateNonConst</span><span class="p">(),</span> <span class="n">rviz_visual_tools</span><span class="o">::</span><span class="n">GREEN</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">publishAxisLabeled</span><span class="p">(</span><span class="n">pose</span><span class="p">.</span><span class="n">pose</span><span class="p">,</span> <span class="s">&quot;goal_2&quot;</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">publishText</span><span class="p">(</span><span class="n">text_pose</span><span class="p">,</span> <span class="s">&quot;Joint Space Goal (2)&quot;</span><span class="p">,</span> <span class="n">rvt</span><span class="o">::</span><span class="n">WHITE</span><span class="p">,</span> <span class="n">rvt</span><span class="o">::</span><span class="n">XLARGE</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">trigger</span><span class="p">();</span>

<span class="cm">/* Wait for user input */</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">prompt</span><span class="p">(</span><span class="s">&quot;Press &#39;next&#39; in the RvizVisualToolsGui window to continue the demo&quot;</span><span class="p">);</span>

<span class="cm">/* Now, we go back to the first goal to prepare for orientation constrained planning */</span>
<span class="n">req</span><span class="p">.</span><span class="n">goal_constraints</span><span class="p">.</span><span class="n">clear</span><span class="p">();</span>
<span class="n">req</span><span class="p">.</span><span class="n">goal_constraints</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="n">pose_goal</span><span class="p">);</span>
<span class="n">context</span> <span class="o">=</span> <span class="n">planner_instance</span><span class="o">-&gt;</span><span class="n">getPlanningContext</span><span class="p">(</span><span class="n">planning_scene</span><span class="p">,</span> <span class="n">req</span><span class="p">,</span> <span class="n">res</span><span class="p">.</span><span class="n">error_code_</span><span class="p">);</span>
<span class="n">context</span><span class="o">-&gt;</span><span class="n">solve</span><span class="p">(</span><span class="n">res</span><span class="p">);</span>
<span class="n">res</span><span class="p">.</span><span class="n">getMessage</span><span class="p">(</span><span class="n">response</span><span class="p">);</span>

<span class="n">display_trajectory</span><span class="p">.</span><span class="n">trajectory</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="n">response</span><span class="p">.</span><span class="n">trajectory</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">publishTrajectoryLine</span><span class="p">(</span><span class="n">display_trajectory</span><span class="p">.</span><span class="n">trajectory</span><span class="p">.</span><span class="n">back</span><span class="p">(),</span> <span class="n">joint_model_group</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">trigger</span><span class="p">();</span>
<span class="n">display_publisher</span><span class="p">.</span><span class="n">publish</span><span class="p">(</span><span class="n">display_trajectory</span><span class="p">);</span>

<span class="cm">/* Set the state in the planning scene to the final state of the last plan */</span>
<span class="n">robot_state</span><span class="o">-&gt;</span><span class="n">setJointGroupPositions</span><span class="p">(</span><span class="n">joint_model_group</span><span class="p">,</span> <span class="n">response</span><span class="p">.</span><span class="n">trajectory</span><span class="p">.</span><span class="n">joint_trajectory</span><span class="p">.</span><span class="n">points</span><span class="p">.</span><span class="n">back</span><span class="p">().</span><span class="n">positions</span><span class="p">);</span>
<span class="n">planning_scene</span><span class="o">-&gt;</span><span class="n">setCurrentState</span><span class="p">(</span><span class="o">*</span><span class="n">robot_state</span><span class="p">.</span><span class="n">get</span><span class="p">());</span>
</pre></div>
</div>
<p>Display the goal state</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">visual_tools</span><span class="p">.</span><span class="n">publishRobotState</span><span class="p">(</span><span class="n">planning_scene</span><span class="o">-&gt;</span><span class="n">getCurrentStateNonConst</span><span class="p">(),</span> <span class="n">rviz_visual_tools</span><span class="o">::</span><span class="n">GREEN</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">trigger</span><span class="p">();</span>

<span class="cm">/* Wait for user input */</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">prompt</span><span class="p">(</span><span class="s">&quot;Press &#39;next&#39; in the RvizVisualToolsGui window to continue the demo&quot;</span><span class="p">);</span>
</pre></div>
</div>
</div>
<div class="section" id="adding-path-constraints">
<h3>Adding Path Constraints<a class="headerlink" href="#adding-path-constraints" title="Permalink to this headline">¶</a></h3>
<p>Let&#8217;s add a new pose goal again. This time we will also add a path constraint to the motion.</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="cm">/* Let&#39;s create a new pose goal */</span>

<span class="n">pose</span><span class="p">.</span><span class="n">pose</span><span class="p">.</span><span class="n">position</span><span class="p">.</span><span class="n">x</span> <span class="o">=</span> <span class="mf">0.32</span><span class="p">;</span>
<span class="n">pose</span><span class="p">.</span><span class="n">pose</span><span class="p">.</span><span class="n">position</span><span class="p">.</span><span class="n">y</span> <span class="o">=</span> <span class="o">-</span><span class="mf">0.25</span><span class="p">;</span>
<span class="n">pose</span><span class="p">.</span><span class="n">pose</span><span class="p">.</span><span class="n">position</span><span class="p">.</span><span class="n">z</span> <span class="o">=</span> <span class="mf">0.65</span><span class="p">;</span>
<span class="n">pose</span><span class="p">.</span><span class="n">pose</span><span class="p">.</span><span class="n">orientation</span><span class="p">.</span><span class="n">w</span> <span class="o">=</span> <span class="mf">1.0</span><span class="p">;</span>
<span class="n">moveit_msgs</span><span class="o">::</span><span class="n">Constraints</span> <span class="n">pose_goal_2</span> <span class="o">=</span>
    <span class="n">kinematic_constraints</span><span class="o">::</span><span class="n">constructGoalConstraints</span><span class="p">(</span><span class="s">&quot;panda_link8&quot;</span><span class="p">,</span> <span class="n">pose</span><span class="p">,</span> <span class="n">tolerance_pose</span><span class="p">,</span> <span class="n">tolerance_angle</span><span class="p">);</span>

<span class="cm">/* Now, let&#39;s try to move to this new pose goal*/</span>
<span class="n">req</span><span class="p">.</span><span class="n">goal_constraints</span><span class="p">.</span><span class="n">clear</span><span class="p">();</span>
<span class="n">req</span><span class="p">.</span><span class="n">goal_constraints</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="n">pose_goal_2</span><span class="p">);</span>

<span class="cm">/* But, let&#39;s impose a path constraint on the motion.</span>
<span class="cm">   Here, we are asking for the end-effector to stay level*/</span>
<span class="n">geometry_msgs</span><span class="o">::</span><span class="n">QuaternionStamped</span> <span class="n">quaternion</span><span class="p">;</span>
<span class="n">quaternion</span><span class="p">.</span><span class="n">header</span><span class="p">.</span><span class="n">frame_id</span> <span class="o">=</span> <span class="s">&quot;panda_link0&quot;</span><span class="p">;</span>
<span class="n">quaternion</span><span class="p">.</span><span class="n">quaternion</span><span class="p">.</span><span class="n">w</span> <span class="o">=</span> <span class="mf">1.0</span><span class="p">;</span>
<span class="n">req</span><span class="p">.</span><span class="n">path_constraints</span> <span class="o">=</span> <span class="n">kinematic_constraints</span><span class="o">::</span><span class="n">constructGoalConstraints</span><span class="p">(</span><span class="s">&quot;panda_link8&quot;</span><span class="p">,</span> <span class="n">quaternion</span><span class="p">);</span>
</pre></div>
</div>
<p>Imposing path constraints requires the planner to reason in the space of possible positions of the end-effector
(the workspace of the robot)
because of this, we need to specify a bound for the allowed planning volume as well;
Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline,
but that is not being used in this example).
We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done
in this volume
when planning for the arm; the bounds are only used to determine if the sampled configurations are valid.</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">req</span><span class="p">.</span><span class="n">workspace_parameters</span><span class="p">.</span><span class="n">min_corner</span><span class="p">.</span><span class="n">x</span> <span class="o">=</span> <span class="n">req</span><span class="p">.</span><span class="n">workspace_parameters</span><span class="p">.</span><span class="n">min_corner</span><span class="p">.</span><span class="n">y</span> <span class="o">=</span>
    <span class="n">req</span><span class="p">.</span><span class="n">workspace_parameters</span><span class="p">.</span><span class="n">min_corner</span><span class="p">.</span><span class="n">z</span> <span class="o">=</span> <span class="o">-</span><span class="mf">2.0</span><span class="p">;</span>
<span class="n">req</span><span class="p">.</span><span class="n">workspace_parameters</span><span class="p">.</span><span class="n">max_corner</span><span class="p">.</span><span class="n">x</span> <span class="o">=</span> <span class="n">req</span><span class="p">.</span><span class="n">workspace_parameters</span><span class="p">.</span><span class="n">max_corner</span><span class="p">.</span><span class="n">y</span> <span class="o">=</span>
    <span class="n">req</span><span class="p">.</span><span class="n">workspace_parameters</span><span class="p">.</span><span class="n">max_corner</span><span class="p">.</span><span class="n">z</span> <span class="o">=</span> <span class="mf">2.0</span><span class="p">;</span>
</pre></div>
</div>
<p>Call the planner and visualize all the plans created so far.</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">context</span> <span class="o">=</span> <span class="n">planner_instance</span><span class="o">-&gt;</span><span class="n">getPlanningContext</span><span class="p">(</span><span class="n">planning_scene</span><span class="p">,</span> <span class="n">req</span><span class="p">,</span> <span class="n">res</span><span class="p">.</span><span class="n">error_code_</span><span class="p">);</span>
<span class="n">context</span><span class="o">-&gt;</span><span class="n">solve</span><span class="p">(</span><span class="n">res</span><span class="p">);</span>
<span class="n">res</span><span class="p">.</span><span class="n">getMessage</span><span class="p">(</span><span class="n">response</span><span class="p">);</span>
<span class="n">display_trajectory</span><span class="p">.</span><span class="n">trajectory</span><span class="p">.</span><span class="n">push_back</span><span class="p">(</span><span class="n">response</span><span class="p">.</span><span class="n">trajectory</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">publishTrajectoryLine</span><span class="p">(</span><span class="n">display_trajectory</span><span class="p">.</span><span class="n">trajectory</span><span class="p">.</span><span class="n">back</span><span class="p">(),</span> <span class="n">joint_model_group</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">trigger</span><span class="p">();</span>
<span class="n">display_publisher</span><span class="p">.</span><span class="n">publish</span><span class="p">(</span><span class="n">display_trajectory</span><span class="p">);</span>

<span class="cm">/* Set the state in the planning scene to the final state of the last plan */</span>
<span class="n">robot_state</span><span class="o">-&gt;</span><span class="n">setJointGroupPositions</span><span class="p">(</span><span class="n">joint_model_group</span><span class="p">,</span> <span class="n">response</span><span class="p">.</span><span class="n">trajectory</span><span class="p">.</span><span class="n">joint_trajectory</span><span class="p">.</span><span class="n">points</span><span class="p">.</span><span class="n">back</span><span class="p">().</span><span class="n">positions</span><span class="p">);</span>
<span class="n">planning_scene</span><span class="o">-&gt;</span><span class="n">setCurrentState</span><span class="p">(</span><span class="o">*</span><span class="n">robot_state</span><span class="p">.</span><span class="n">get</span><span class="p">());</span>
</pre></div>
</div>
<p>Display the goal state</p>
<div class="highlight-c++"><div class="highlight"><pre><span class="n">visual_tools</span><span class="p">.</span><span class="n">publishRobotState</span><span class="p">(</span><span class="n">planning_scene</span><span class="o">-&gt;</span><span class="n">getCurrentStateNonConst</span><span class="p">(),</span> <span class="n">rviz_visual_tools</span><span class="o">::</span><span class="n">GREEN</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">publishAxisLabeled</span><span class="p">(</span><span class="n">pose</span><span class="p">.</span><span class="n">pose</span><span class="p">,</span> <span class="s">&quot;goal_3&quot;</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">publishText</span><span class="p">(</span><span class="n">text_pose</span><span class="p">,</span> <span class="s">&quot;Orientation Constrained Motion Plan (3)&quot;</span><span class="p">,</span> <span class="n">rvt</span><span class="o">::</span><span class="n">WHITE</span><span class="p">,</span> <span class="n">rvt</span><span class="o">::</span><span class="n">XLARGE</span><span class="p">);</span>
<span class="n">visual_tools</span><span class="p">.</span><span class="n">trigger</span><span class="p">();</span>
</pre></div>
</div>
</div>
</div>
<div class="section" id="the-launch-file">
<h2>The Launch File<a class="headerlink" href="#the-launch-file" title="Permalink to this headline">¶</a></h2>
<p>The entire launch file is <a class="reference external" href="https://github.com/ros-planning/moveit_tutorials/tree/melodic-devel/doc/motion_planning_api/launch/motion_planning_api_tutorial.launch">here</a> on GitHub. All the code in this tutorial can be compiled and run from the moveit_tutorials package.</p>
</div>
</div>


            
            <div class="admonition note">
              <p class="first admonition-title">Open Source Feedback</p>
              <p class="last">See something that needs improvement? Please open a pull request on this <a href="https://github.com/ros-planning/moveit_tutorials/blob/melodic-devel/doc/motion_planning_api/motion_planning_api_tutorial.rst" class="fa fa-github"> GitHub page</a></p>
            </div>
            
           </div>
          </div>
          <footer>
  
    <div class="rst-footer-buttons" role="navigation" aria-label="footer navigation">
      
        <a href="../motion_planning_pipeline/motion_planning_pipeline_tutorial.html" class="btn btn-neutral float-right" title="Motion Planning Pipeline" accesskey="n">Next <span class="fa fa-arrow-circle-right"></span></a>
      
      
        <a href="../planning_scene_ros_api/planning_scene_ros_api_tutorial.html" class="btn btn-neutral" title="Planning Scene ROS API" accesskey="p"><span class="fa fa-arrow-circle-left"></span> Previous</a>
      
    </div>
  

  <hr/>

  <div role="contentinfo">
    <p>
    </p>
  </div>
  Built with <a href="http://sphinx-doc.org/">Sphinx</a> using a <a href="https://github.com/snide/sphinx_rtd_theme">theme</a> provided by <a href="https://readthedocs.org">Read the Docs</a>.

</footer>
        </div>
      </div>

    </section>

  </div>
  


  

    <script type="text/javascript">
        var DOCUMENTATION_OPTIONS = {
            URL_ROOT:'../../',
            VERSION:'Kinetic',
            COLLAPSE_INDEX:false,
            FILE_SUFFIX:'.html',
            HAS_SOURCE:  true
        };
    </script>
      <script type="text/javascript" src="../../_static/jquery.js"></script>
      <script type="text/javascript" src="../../_static/underscore.js"></script>
      <script type="text/javascript" src="../../_static/doctools.js"></script>

  

  
  
    <script type="text/javascript" src="../../_static/js/theme.js"></script>
  

  
  
  <script type="text/javascript">
      jQuery(function () {
          SphinxRtdTheme.StickyNav.enable();
      });
  </script>
   

</body>
</html>